//**********************************************************************
//*
//*       Copyright 2007 - FRC Team 1675 - Ultimate Protection squad
//*
//*       Autoflex.c
//*
//*       Contains module required for autoflex autonomous programming tool.
//*       Add a call to 'autoflex playback to the autonomous procedure in
//*       user routines_fast.c
//*       Add a call to 'autoflex_recorder()' in process_uP_data() in 
//*       user_routines.c
//*       add include "autoflex.h" to user_routines.c and user_routines_fast.c
//*       and main.c
//**********************************************************************
#include "AutoFlex.h"
#include "EEPROM.h"
#include "user_routines.h"
#if FRC
#include <stdio.h>
#else
#include "printf_lib.h"
#endif

/*** DEFINE USER VARIABLES AND INITIALIZE THEM HERE ***/
unsigned int  tenths_of_secs = 0;

//--------------------------------------------------------------
void autoflex_playback(void)
{
static unsigned int address = START_OF_AUTO_PROGRAM;
static unsigned char tick_counter   = 0;
static unsigned char temp_auto_command_1 = 127;
static unsigned char temp_auto_command_2 = 127;
static unsigned char temp_auto_command_3 = 127;
static unsigned char temp_auto_command_4 = 127;
static unsigned char temp_auto_command_5 = 127;
static unsigned char temp_auto_command_6 = 127;

/****************************************************************************************/
//   run the autonomous commands for only 15 seconds then stop and wait for driver mode
//   once autonomous runs you have to do a power on reset to run it again.
//***************************************************************************************
  if (tenths_of_secs < TIME_LIMIT-1)  // read the table for 14.9 second
  {
    /* This code creates a counter that increments approximately once per tenth of a second */
    tick_counter++;
    if (tick_counter >= TICKS_PER_TENTH) // every tenth of second read next set of commands
    {
      tick_counter = 0;
      tenths_of_secs++;
  
      temp_auto_command_1 = read_EEPROM(address++);
      temp_auto_command_2 = read_EEPROM(address++);
      temp_auto_command_3 = read_EEPROM(address++);
      temp_auto_command_4 = read_EEPROM(address++);

#if (NUM_OF_INPUTS > 4)
      temp_auto_command_5 = read_EEPROM(address++);
#endif
#if (NUM_OF_INPUTS > 5)
      temp_auto_command_6 = read_EEPROM(address++);
#endif     
    }
  }
  else  // times up..neutralize all commands
  {
    temp_auto_command_1 = temp_auto_command_2 = AUTO_NEUTRAL_PWM_VALUE;
	temp_auto_command_3 = temp_auto_command_4 = AUTO_NEUTRAL_PWM_VALUE ;

#if (NUM_OF_INPUTS > 4)
    temp_auto_command_5 = AUTO_NEUTRAL_PWM_VALUE;
#endif
#if (NUM_OF_INPUTS > 5)
    temp_auto_command_6 = AUTO_NEUTRAL_PWM_VALUE;
#endif 
    
    address = START_OF_AUTO_PROGRAM;  
  }

  AUTO_COMMAND1 = temp_auto_command_1;
  AUTO_COMMAND2 = temp_auto_command_2;
  AUTO_COMMAND3 = temp_auto_command_3;
  AUTO_COMMAND4 = temp_auto_command_4;
#if (NUM_OF_INPUTS > 4)
    AUTO_COMMAND5 = temp_auto_command_5;
#endif
#if (NUM_OF_INPUTS > 5)
    AUTO_COMMAND6 = temp_auto_command_6;
#endif 
  
  Default_Routine(); // call routine where all your command functions are.

}
/******************************************************************************/
// This function will record command for 15 seconds from the time
// the Auto_Record button is pressed (see autoflex.h)
// it is suggested that fresh batteries be used for each recording session
// FRC: Make sure competition port dongle is set to practice (not Autonomous) and
// cycle power to reset computer before recording
/******************************************************************************/
void autoflex_recorder(void)
{
static unsigned int address = START_OF_AUTO_PROGRAM;
static unsigned int tenths_of_secs_recorder = 0;
static unsigned int tick_counter_recorder = 0;
static unsigned char capture_flag = FALSE;

if (WRITE_ENABLED) 
{
  if ((AUTO_RECORD) && (capture_flag == FALSE))
  {
    capture_flag = TRUE;
    address = START_OF_AUTO_PROGRAM; // initialize EEPROM start address
    printf ("programming\r\n");
  }

  if ((capture_flag == TRUE) && (tenths_of_secs_recorder < TIME_LIMIT-1))
  {
    tick_counter_recorder++;
    if (tick_counter_recorder >=TICKS_PER_TENTH)
    {
      tenths_of_secs_recorder++;
      tick_counter_recorder =0;

      write_EEPROM ((unsigned char)AUTO_COMMAND1,address++);
      write_EEPROM ((unsigned char)AUTO_COMMAND2,address++);
      write_EEPROM ((unsigned char)AUTO_COMMAND3,address++);
      write_EEPROM ((unsigned char)AUTO_COMMAND4,address++);

#if (NUM_OF_INPUTS > 4)
      write_EEPROM ((unsigned char)AUTO_COMMAND5,address++);
#endif
#if (NUM_OF_INPUTS > 5)
      write_EEPROM ((unsigned char)AUTO_COMMAND6,address++);
#endif     

    }
  }
  else
  {
    if ((!AUTO_RECORD) && (capture_flag == TRUE))
    {    
      write_EEPROM (AUTO_NEUTRAL_PWM_VALUE,address++);
      write_EEPROM (AUTO_NEUTRAL_PWM_VALUE,address++);
      write_EEPROM (AUTO_NEUTRAL_PWM_VALUE,address++);
      write_EEPROM (AUTO_NEUTRAL_PWM_VALUE,address++);

#if (NUM_OF_INPUTS > 4)
      write_EEPROM (AUTO_NEUTRAL_PWM_VALUE,address++);
#endif
#if (NUM_OF_INPUTS > 5)
      write_EEPROM (AUTO_NEUTRAL_PWM_VALUE,address++);
#endif     
       
      capture_flag = FALSE;
      tick_counter_recorder = 0;
      tenths_of_secs_recorder = 0;
      address = START_OF_AUTO_PROGRAM;  // reinitialize EEPROM start Address
      printf ("programming complete\r\n");
    }
  }
}
}
/******************************************************************************/
//
//   rewind_autoflex_payback()
//
//   rewinds the playback timer so it will run from beginning
//   when we enter autonomous mode. Add a call to this in Main 
//   as an else block with the test for autonomous mode.
//      if (autonomous_mode)            /* DO NOT CHANGE! */
//      {
//        User_Autonomous_Code();        /* You edit this in user_routines_fast.c */
//      }
//      else
//      {
//        rewind_autoflex_playback(); 
//     }
//***************************************************************

void rewind_autoflex_playback(void)
{
  tenths_of_secs = 0;
}
